#include "stm32f10x.h"

#include "delay.h"
#include "gpio.h"
#include "pid.h"
#include "pwm.h"
#include "adc.h"
#include "encoder.h"
#include "move.h"
#include "oled.h"
#include <stdio.h>
#include "Serial.h"

#define X_BUFFER_SIZE 5
int targetspeed=200;
float Velcity_Kp=1.5,  Velcity_Ki=0.000000035,  Velcity_Kd=0.00000007; 

u16 encoder_A,encoder_B;
int Velocity_PWM1,Velocity_PWM2;
u16 adcx;
float vcc;
u16 x=0,d=0;
u16 x_zero_duration = 0;


int x_buffer[X_BUFFER_SIZE];
int x_buffer_index = 0;
int sum = 0;
int count = 0;
int x_average=0;
int i=0;

int main(void)
{
    SystemInit(); //ÅäÖÃÏµÍ³Ê±ÖÓÎª72M   
    delay_init();    //ÑÓÊ±º¯Êý³õÊ¼»¯
    Serial_Init(57600);
    OLED_Init();
    adc_Init();				//ADC1µÄ³õÊ¼»¯   
    PWM_Int(1000-1,7.2);      //³õÊ¼»¯pwmÊä³ö 72000 000 /7199+1=10000 
    Encoder_Init_Tim2();
    Encoder_Init_Tim4();
    OLED_ShowString(1,1,"V:");
    OLED_ShowString(2,1,"S:");


//    int x_zero_duration = 0;
    while(1)
    {
//		delay_ms(300);
		/**************************************************************************/
	
    
    if (Serial_RxFlag == 1)
{
//			OLED_ShowString(4, 1, "                ");
//			OLED_ShowString(4, 1, Serial_RxPacket);
      if(Serial_RxPacket[0]=='x')
      {
//        OLED_ShowString(4, 1, Serial_RxPacket);
         
         x= (Serial_RxPacket[1] - '0') * 100 + (Serial_RxPacket[2] - '0') * 10 + (Serial_RxPacket[3] - '0');
		 d= (Serial_RxPacket[4] - '0') * 100 + (Serial_RxPacket[5] - '0') * 10 + (Serial_RxPacket[6] - '0');
      }
       if(Serial_RxPacket[0]=='d')
      {
//        OLED_ShowString(4, 5, Serial_RxPacket);
//		  d= (Serial_RxPacket[1] - '0') * 100 + (Serial_RxPacket[2] - '0') * 10 + (Serial_RxPacket[3] - '0');
      }
  }
OLED_ShowNum(1,3,vcc,3);
    OLED_ShowNum(2,3,encoder_A*2.2,5);
    OLED_ShowNum(3,1,Velocity_PWM1,5);
    OLED_ShowNum(3,7,Velocity_PWM2,5);
    OLED_ShowNum(4,7,x,3);
    OLED_ShowNum(4,11,d,3);
//		if(x==0)
//		{
//			x_zero_duration += 300;
//			if (x_zero_duration >= 7000)
//			{
//				turn();
//				x_zero_duration = 0;
//			}
//			else
//			{
//				 if(x<100)
//					x=100;
//				else if(x>600)
//						x=600;
//		        if(d>100)
//					stop();
//				else track((x-270)/6);
//				
//				x_zero_duration = 0;
//			}
//		}
		if (x != 0)
     {
            x_buffer[x_buffer_index] = x;
            x_buffer_index = (x_buffer_index + 1) % X_BUFFER_SIZE;
        
	
        
	 track((x - 270) / 6);
         
	}
	else if (x==0){
			for ( i = 0; i < X_BUFFER_SIZE; i++)
        {
            if (x_buffer[i] != 0)
            {
                sum += x_buffer[i];
                count++;
            }
        }
			x_average = (count > 0) ? (sum / count) : 0;
			track((x_average - 270) / 6);
		}
		delay_ms(100);
       
/**************************************************************************/
		
		
    

        
        
//        turn();
//       run();
//  right();
//  delay_ms(5000);
//  left();
//  delay_ms(10000);
  Serial_RxFlag = 0;
    }
}